#include <Servo.h>
#include <Wire.h>
#include "SDuFAT.h"

//adresses
int HMC6352Address = 0x42;
int slaveAddress;
#define GPM 0x68 // GPM I2C Address                                        // Global variable
#define MEM_PW 8 //patte d'alimentation SD

//constantes de réglage
int errcapadm=10;
int neutreservo=91;
int courseservo=45;
int nbrectrlcap=10;
int tempslissage=100;
int coefflargroute=5;
long largroutemin=20;
long largroutemax=50000;
float pi=3.141592654;
long rmoyterre=6371030;
float VminGPS=1;
int AngleInit=15;//angle de servo pour l'initialisation
int tempsinit=1000;//fréquence de prélèvement des caps pour l'initialisation
int nbval=5;//nbr de valeur pour lesquelles on considère que le navire ne peut plus évoluer
float diffmin=0.11;//difference d'angle minimale
int tempsmes=5000;//temps d'attente entre l'acquisition du premier cap et le second (histoire que le navire reprenne un peu de vitesse)
float alpha=25;
float memoireboussole=0;
float memoireGPS=0;
float beta=15;
float dt=1;

//Variables globales
byte Address;                                         // Global variable
float Data;    
char chaine[10];                                    // array to hold the data for the ftoa fuction   
int posservo=90;
int capboussoleasuivre;
boolean correctderiveactive=false;
long ecartref;
int WPlatdegres[10];
float WPlatminutes[10];
char WPlatletter[10];
int WPlongdegres[10];
float WPlongminutes[10];
char WPlongletter[10];
long WPvalidate[10];
float targetWPlat;
float targetWPlong;
float previousWPlat;
float previousWPlong;
float guessWPlat;
float guessWPlong;
int targetWP=0;
long targetWPvalidate;
int cmdservoderive=0;

//Objets
Servo servo;  // create servo object to control a servo
 
void setup()
{
  servo.attach(2);  // attaches the servo on pin 2 to the servo object
  //alimentation boussole:
  pinMode(16, OUTPUT);
  digitalWrite(16, LOW);
  pinMode(17, OUTPUT);
  digitalWrite(17, HIGH);
  slaveAddress = HMC6352Address >> 1;   // This results in 0x21 as the address to pass to TWI
  Wire.begin();
  // on my MicroSD Module the power comes from a digital pin
  // I activate it at all times
  pinMode(MEM_PW, OUTPUT);
  digitalWrite(MEM_PW, HIGH);
  
  Serial.begin(9600);
  
  cmdservoderive=neutreservo;
  
  //Liste des waypoints:
  //WP1:
  WPlatdegres[0]=48;
  WPlatminutes[0]=25.135;
  WPlatletter[0]='N';
  WPlongdegres[0]=4;
  WPlongminutes[0]=28.441;
  WPlongletter[0]='O';
  WPvalidate[0]=15;
  //WP2:
  WPlatdegres[1]=48;
  WPlatminutes[1]=25.082;
  WPlatletter[1]='N';
  WPlongdegres[1]=4;
  WPlongminutes[1]=28.432;
  WPlongletter[1]='O';
  WPvalidate[1]=15;
}

int GetDouble(){				      // Get double register value from GPM

  int Value = 0; 
  byte H_Byte = 0;
  byte L_Byte = 0;

  Wire.beginTransmission(GPM);
  Wire.send(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 2);			      // Request double register
  while(Wire.available() < 2);			      // Wait for double byte
  H_Byte = Wire.receive();                            // Store one
  L_Byte = Wire.receive();                            // Store two

  Value = (H_Byte * 10) + L_Byte;                     // Adjust for one byte

  return(Value);	                          
}


int GetSingle(){				      // Get single register value from GPM

  int Value = 0; 

  Wire.beginTransmission(GPM);
  Wire.send(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 1);			      // Request register
  while(Wire.available() < 1);			      // Wait for single byte
  Value = Wire.receive();                             // and store

  return(Value);	                          
}

void GPSHeading(float* cap){ //renvoie le cos et le sin du cap
  float heading;
  Address = 44;                                       // Point to Heading hundreds and tens
  Data = GetDouble();                                 // Read registers from GPM
  heading=10*Data;
  Address = 46;                                       // Point to Heading units
  Data = GetSingle();                                 // Read registers from GPM
  heading=heading+Data;
  Address = 47;                                       // Point to Heading tenths
  Data = GetSingle();                               
  heading=heading+(Data/10);
  heading = radians(heading);
  cap[0]= cos(heading);
  cap[1]= sin(heading);
}

float GPSSpeed(){ //renvoie la vitesse sur un flottant
  float vitesse;
  Address = 52;                                       // Point to Speed hundreds and tens
  Data = GetDouble();                                 // Read registers from GPM
  vitesse=Data*10;
  Address = 54;                                       // Point to Speed units
  Data = GetSingle();                                 // Read registers from GPM
  vitesse=vitesse+Data;
  Address = 55;                                       // Point to Speed units
  Data = GetSingle();                                 // Read registers from GPM
  vitesse=vitesse+(Data/10);
  return vitesse;
}

float GPSLatitude(){ //renvoie la latitude en radians dans notre système (-pi/2 ; pi/2)
  float latitude;
  float minutes;
  char letter;
  Address = 14;                                       // Point to Latitude tens
  Data = GetSingle();                                 // Read registers from GPM
  latitude= Data*10;
  Address = 15;                                       // Point to Latitude units
  Data = GetSingle();                                 // Read registers from GPM
  latitude= latitude+Data;
  Address = 16;                                       // Point to Latitude minutes tens
  Data = GetSingle();                                 // Read registers from GPM
  minutes= Data*10;
  Address = 17;                                       // Point to Latitude minutes units
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data;
  Address = 18;                                       // Point to Latitude minutes tenths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10;
  Address = 19;                                       // Point to Latitude minutes hundredths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/100;
  Address = 20;                                       // Point to Latitude minutes thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/1000;
  Address = 21;                                       // Point to Latitude minutes ten thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10000;
  latitude=latitude+(minutes/60);
  Address = 22;                                       // Point to Latitude character register
  Data = GetSingle();                                 // Read registers from GPM
  letter=Data;
  if (letter=='S'){                 // Prise en conpte de la lettre et passage dans notre système
    latitude=-latitude;
  }
  return radians(latitude);
}

float GPSLongitude(){  //renvoie la longitude en radians dans notre système (-pi ; pi)
  float longitude;
  float minutes;
  char letter;
  Address = 23;                                       // Point to Longitude hundreds
  Data = GetSingle();                                 // Read registers from GPM
  longitude= Data*100;
  Address = 24;                                       // Point to Longitude tens
  Data = GetSingle();                                 // Read registers from GPM
  longitude= longitude+Data*10;
  Address = 25;                                       // Point to Longitude units
  Data = GetSingle();                                 // Read registers from GPM
  longitude= longitude+Data;
  Address = 26;                                       // Point to Longitude minutes tens
  Data = GetSingle();                                 // Read registers from GPM
  minutes= Data*10;
  Address = 27;                                       // Point to Longitude minutes units
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data;
  Address = 28;                                       // Point to Longitude minutes tenths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10;
  Address = 29;                                       // Point to Longitude minutes hundredths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/100;
  Address = 30;                                       // Point to Longitude minutes thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/1000;
  Address = 31;                                       // Point to Longitude minutes ten thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10000;
  longitude=longitude+(minutes/60);
  Address = 32;                                       // Point to Longitude character register
  Data = GetSingle();                                 // Read registers from GPM
  letter=Data;
  if (letter=='E'){                                    // Prise en conpte de la lettre et passage dans notre système
    longitude=-longitude;
  }
  return radians(longitude);
}


void Boussole(float* heading)
{
  byte headingData[2];
  int i;
  float headingValue;
  Wire.beginTransmission(slaveAddress);
  Wire.send("A");              // The "Get Data" command
  Wire.endTransmission();
  delay(10);                   // The HMC6352 needs at least a 70us (microsecond) delay
  // after this command.  Using 10ms just makes it safe
  // Read the 2 heading bytes, MSB first
  // The resulting 16bit word is the compass heading in 10th's of a degree
  // For example: a heading of 1345 would be 134.5 degrees
  Wire.requestFrom(slaveAddress, 2);        // Request the 2 byte heading (MSB comes first)
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  // Put the MSB and LSB together
  headingValue = headingValue/ 10; //cap sur 360°
  headingValue=360-headingValue; //retournement de la boussole
  headingValue = radians(headingValue);
  heading[0]= cos(headingValue);
  heading[1]= sin(headingValue);
} 


void BoussoleLissee(float* caplisse){
  float cap[2];
  float sinus[20];   //nbre pts lissage
  float cosinus[20];  //nbre pts lissage
  int nbrepointslissage=20; //nbre pts lissage DOIT ETRE EGAL AUX VALEURS DANS LES TABLEAUX
  float sommecos=0;
  float sommesin=0;
  
  for (int i=0; i<nbrepointslissage; i++){//boucle pendant laquelle on prélève un cap tous les tempslissage; on prélève nbrepointslissage valeurs.
    Boussole(cap);
    cosinus[i]=cap[0];
    sinus[i]=cap[1];
    delay(tempslissage);
   }
  for (int i=0; i<nbrepointslissage; i++){//somme de tous les caps prélevés ci-dessus
    sommecos=sommecos+cosinus[i];
    sommesin=sommesin+sinus[i];
   } 
  caplisse[0] = sommecos/nbrepointslissage;
  caplisse[1] = sommesin/nbrepointslissage;
 }


char *ftoa(char *a, double f, int precision) {
  long p[] = {0,10,100,1000,10000,100000,1000000,10000000,100000000};
  
  char *ret = a;
  long heiltal = (long)f;
  itoa(heiltal, a, 10);
  while (*a != '\0') a++;
  *a++ = '.';
  long desimal = abs((long)((f - heiltal) * p[precision]));
  itoa(desimal, a, 10);
  return ret;
} 

long dist(float latitudedep, float longitudedep, float latitudear, float longitudear){ // Retourne la distance en mètres entre 2 points GPS avec une erreur maximale de 0.2%
  float latitude;
  float longitude;
  float a;
  float c;
  float d;
  float e;
  long distance;
  latitude= (abs(latitudedep-latitudear));
  longitude= min(min(abs(longitudedep-longitudear), abs((2*pi+longitudedep)-longitudear)), abs(longitudedep-(longitudear+2*pi)));
  a = (square(sin(latitude/2.0))) + cos(latitudedep)*cos(latitudear)*(square(sin(longitude/2.0)));
  d=sqrt(a);
  e=sqrt(1-a);
  c = 2*atan2(d, e);
  distance= rmoyterre*c;
  return distance;
}

void GPSHeadingTh(float latitudedep, float longitudedep, float latitudear, float longitudear, float* cap){  // Retourne le cos le sin du cap entre 2 points GPS
  float heading;
  float a;
  float b;
  float c;
  float d;
  float longitude;
  longitude= min(min(abs(longitudedep-longitudear), abs((2*pi+longitudedep)-longitudear)), abs(longitudedep-(longitudear+2*pi)));
  a=sin(longitude)*cos(latitudear);
  c=cos(latitudedep)*sin(latitudear);
  d=sin(latitudedep)*cos(latitudear)*cos(longitude);
  b=c-d;
  heading=atan2(a,b);
  if (sin(longitudear-longitudedep)>0){                    //passage de [-180°; 180°] à [0°; 360°] pour rejoindre la convention des caps d'un compas.
   heading=2*pi-abs(heading); 
  }
  cap[0]=cos(heading);
  cap[1]=sin(heading);
}


float ConversionLatitude(int degres, float minutes, char letter){
  float latitude;
  latitude= degres+ minutes/60;
  if (letter=='S'){                                      // Prise en conpte de la lettre et passage dans notre système
    latitude=-latitude;
  }
  return radians(latitude);
}

float ConversionLongitude(int degres, float minutes, char letter){
  float longitude;
  longitude= degres+ minutes/60;
  if (letter=='E'){                                    // Prise en conpte de la lettre et passage dans notre système
    longitude=-longitude;
  }
  return radians(longitude);
}

void servocmd(int angle){                     //verifie que l'on ne depasse pas les butées de commande du servo
  if (angle<=neutreservo+courseservo && angle>=neutreservo-courseservo){
    servo.write(angle);
  }
  else{
    if(angle>neutreservo+courseservo){
      servo.write(neutreservo+courseservo);
    }
    if(angle<neutreservo-courseservo){
      servo.write(neutreservo-courseservo);
    }
  }
   Serial.print("Angle servo:");
   Serial.println(angle);
}

float ecartroute(float latitudedep, float longitudedep, float latitudear, float longitudear){ //retourne l'écart à la route définie par les 2 points GPS en m sur un long
float headingroute[2];
GPSHeadingTh( latitudedep, longitudedep, latitudear, longitudear, headingroute);
float proj[2];
proj[0]=-headingroute[1];
proj[1]=headingroute[0];
float headingtocurrentpoint[2];
GPSHeadingTh(latitudedep, longitudedep, GPSLatitude(), GPSLongitude(), headingtocurrentpoint);
float distancedone=dist(latitudedep, longitudedep, GPSLatitude(), GPSLongitude());        //distance parcourue
float ecart;
ecart= distancedone*(proj[0]*headingtocurrentpoint[0]+proj[1]*headingtocurrentpoint[1]);
   Serial.print("Ecart: ");
   Serial.println(ecart);
return ecart;
}


boolean SuiviRoute(float latitudedep, float longitudedep, float latitudear, float longitudear){     // cap de depart boussole a definir au debut
  //calcul des variables nécessaires pour la fonction:
  long longroute= dist(latitudedep, longitudedep, latitudear, longitudear);
  long largroute= longroute/coefflargroute;
  long ecart;
  float theoricalheading[2];
  GPSHeadingTh( GPSLatitude(), GPSLongitude(), latitudear, longitudear, theoricalheading);
  float currentheading[2];
  GPSHeading(currentheading);
  float capboussole[2];
  BoussoleLissee(capboussole);
  float erreur;
 //Calcul de la largeur de la route
 if (largroute>largroutemax){
   largroute=largroutemax;
 }
  if (largroute<largroutemin){
   largroute=largroutemin;
 }
 //Vérification que le bateau est bien présent à l'intérieur du couloir de la route définie, calcul de l'écart à la route:
 ecart=ecartroute(latitudedep, longitudedep, latitudear, longitudear);
 if(abs(ecart)<largroute/2){                                                              //bateau présent dans le couloir          
    if((abs(ecart)<largroute/3) && correctderiveactive==false){
     ecartref=ecart;
     if (GPSSpeed()<=VminGPS){ // asservir sur cap boussole.
       memoireGPS=0;
       if (capboussole[0]*theoricalheading[0]+capboussole[1]*theoricalheading[1]>0){
         erreur=capboussole[0]*theoricalheading[1]-capboussole[1]*theoricalheading[0];
         memoireboussole=memoireboussole+dt*erreur;
         if (memoireboussole>courseservo){  //borner la mémoire
           memoireboussole=courseservo;
         }
         if (memoireboussole<-courseservo){ //borner la mémoire
           memoireboussole=-courseservo;
         }
         servocmd(neutreservo-(alpha*erreur+beta*memoireboussole));
              Serial.println("asservi sur boussole");
       }
       else{
         memoireboussole=0;
         if(capboussole[0]*theoricalheading[1]-capboussole[1]*theoricalheading[0]>0){
           servocmd(neutreservo-courseservo);       //virer sur tribord
           Serial.println("virer sur tribord boussole");
         }
         else{
           servocmd(neutreservo+courseservo);       //virer sur babord  
           Serial.println("virer sur babord boussole");
         }
       }
     }
     else{ // asservir sur cap GPS
       memoireboussole=0;
       if (currentheading[0]*theoricalheading[0]+currentheading[1]*theoricalheading[1]>0){
         erreur=currentheading[0]*theoricalheading[1]-currentheading[1]*theoricalheading[0];
         memoireGPS=memoireGPS+dt*erreur;
         if (memoireGPS>courseservo){  //borner la mémoire
           memoireGPS=courseservo;
         }
         if (memoireGPS<-courseservo){ //borner la mémoire
           memoireGPS=-courseservo;
         }
         servocmd(neutreservo-(alpha*erreur+beta*memoireGPS));
               Serial.println("asservi sur gps");
       }
       else{
         memoireboussole=0;
         if(currentheading[0]*theoricalheading[1]-currentheading[1]*theoricalheading[0]>0){
           servocmd(neutreservo-courseservo);       //virer sur tribord
           Serial.println("virer sur tribord gps");
         }
         else{
           servocmd(neutreservo+courseservo);       //virer sur babord
           Serial.println("virer sur babord gps");  
         }
       }
     }
    }
   else{// on est en dehors de la route de largeur 2/3 largroute ou bien la correction est active
     correctderiveactive=true;
     memoireboussole=0;
     memoireGPS=0;
   Serial.println("Correction derive active");
     if (abs(ecart)<largroute/6){ //dérive corrigée
       correctderiveactive=false;
       cmdservoderive=neutreservo;
     }
     else{ // correction de la dérive
        if (abs(ecart)<abs(ecartref)){
           ecartref=ecart; 
        }
        else{ // Ramener le bateau en milieu de route.
          if(ecart<0){      //virer sur tribord
            cmdservoderive--;
            servocmd(cmdservoderive);
                 Serial.println("derive: aller tribord");
          }
          else{          //virer sur tribord
            cmdservoderive++;
            servocmd(cmdservoderive);
                 Serial.println("derive: aller babord");
          }
         }
       } 
   }  
  return true;  
 }
 Serial.println("Impossible");
 memoireboussole=0;
 memoireGPS=0;
 return false;
}

float diffcap(){
  float a[2];
  float b[2];
  float det;

  delay(tempsinit);
  BoussoleLissee(a);
  delay(tempsinit);
  BoussoleLissee(b);
  det = abs(a[0]*b[1]-a[1]*b[0]);
   
    return det;
  }
  
void capimmobile(float* c){ //fonction de détection d'immobilisation du navire
  boolean cond=true;
  float diff=0;
  int f=0;
  Serial.println("debut boucle");

    for(int i=0;cond==true;i++){
    diff=diffcap();
  
    Serial.print("diff cap : ");
    Serial.println(diff);  
    
    if (diff <= diffmin){
        f++;  
     Serial.print("f : ");
     Serial.println(f); 
          }
     else{f=0;}         
                 
     if(f>=nbval){
        cond=false;
      GPSHeading(c); //alors on fixe c au cap 
      //  Serial.print("cap : ");
      //  Serial.println(c[0]);
      //  Serial.println(c[1]);
      f=0; }
        
        else{}
      }
 }
 
    
void initialisation(float* capvent){
 float cap1[2];
 float cap2[2]; 
  servo.write(neutreservo+AngleInit); //on fait tourner le bateau avec un angle de barre faible AngleInit dans le sens des aiguilles d'une montre
  capimmobile(cap1);
  Serial.print("cap 1 : ");
  Serial.println(cap1[0]);
  Serial.println(cap1[1]);
  servo.write(neutreservo-AngleInit); //le servo se met dans la position inverse  
  delay(tempsmes);//on attend un moment que le navire reprenne de la vitesse 
  capimmobile(cap2);
  Serial.print("cap 2 : ");
  Serial.println(cap2[0]);
  Serial.println(cap2[1]);
  servo.write(neutreservo);
  
  float v0 = cap1[0] + cap2[0];
  //Serial.println(v0);
  float v1 = cap1[1] + cap2[1];
  //Serial.println(v1);
  capvent[0] = v0/(sqrt((v0*v0)+(v1*v1)));
  capvent[1] = v1/(sqrt((v0*v0)+(v1*v1)));
}

void loop(){
//  Serial.print("Speed: ");                               //test vitesse GPS
//  Serial.println(GPSSpeed());
//  Serial.print("100000*longitude: ");                    // test longitude GPS
//  Serial.println(100000*GPSLongitude());
//  Serial.print("100000*latitude: ");                     // test latitude GPS
//  Serial.println(100000*GPSLatitude());
//  Serial.println(1000*ConversionLongitude(45, 25.2356, 'E')); //Test conversion coordonnées GPS
//  Serial.println(1000*ConversionLatitude(45, 25.2356, 'N'));
//  Serial.println(dist(ConversionLatitude(48, 22.666, 'N'), ConversionLongitude(4, 29.628, 'O'), ConversionLatitude(48, 18.701, 'N'), ConversionLongitude(4, 29.947, 'O'))); //test distance
//  Serial.println(GPSHeadingTh(ConversionLatitude(48, 11.706, 'N'), ConversionLongitude(4, 53.990, 'O'), ConversionLatitude(43, 34.831, 'N'), ConversionLongitude(8, 20.909, 'O'))); // test cap théorique
//SuiviRoute(ConversionLatitude(WPlatdegres[0], WPlatminutes[0], WPlatletter[0]), ConversionLongitude(WPlongdegres[0], WPlongminutes[0], WPlongletter[0]), ConversionLatitude(WPlatdegres[1], WPlatminutes[1], WPlatletter[1]),  ConversionLongitude(WPlongdegres[1], WPlongminutes[1], WPlongletter[1]));
//Serial.println(alpha);
//Serial.println(beta);
float a[2];
//capimmobile(a);
//GPSHeading(a);
initialisation(a);
  Serial.print("a[0] : ");
  Serial.println(a[0]);
  Serial.print("a[1] : ");
  Serial.println(a[1]);
//Serial.println(diffcap());
delay(1000);
}
